//
// Created by l on 2021/11/5.
//
#ifndef UAV_ASTAR_H
#define UAV_ASTAR_H


#include<iostream>
#include<queue>
#include "RMap.h"

using namespace std;

//
//point class that storage every point in map array
//

class AStar {
public:

    static vector<point> Acalc(RMap rmap, int, int);

    static int manhattanDist(const point &a, const point &b) {

        int dx = abs(a.x * 10 - b.x * 10);
        int dy = abs(a.y * 10 - b.y * 10);

        return dx + dy;
    }

    static int calcG(const point &a, const point &b) {
        int mdist = manhattanDist(a, b);

        if (mdist == 10) {
            return 10;
        } else if (mdist == 20) {
            return 14;
        }
        return 0;
    }
};

vector<point> AStar::Acalc(RMap rmap, int sx, int sy) {

    const int next_pos[8][2] = {{0,  1},
                                {-1, 0},
                                {-1, 1},
                                {1,  0},
                                {1,  1},
                                {0,  -1},
                                {-1, -1},
                                {1,  -1}};

    vector<point> openlist;
    vector<point> closelist;
    vector<point> resPath;

    //把起始格添加到开启列表
    openlist.emplace_back(sx, sy, nullptr, 0, 0, 0);

    while (!openlist.empty()) {

        point cur_point(sx, sy);
        int F = 99999;

        //寻找开启列表中F值最低的格子。我们称它为当前格。
        for (auto t: openlist) {
            if (F > t.F) {
                F = t.F;
                cur_point = t;
            }
        }

        //把当前格切换到关闭列表。
        auto it = openlist.begin();
        while (it != openlist.end()) {
            if ((*it) == cur_point) {
                closelist.push_back((*it));
                it = openlist.erase(it);
                break;
            } else {
                it++;
            }
        }

        //找(x,y)的点，加入openlist
        //对相邻的8格中的每一个？
        for (auto &i: next_pos) {
            int nextx = cur_point.x + i[0];
            int nexty = cur_point.y + i[1];
            point next_point(nextx, nexty, nullptr, 0, 0, 0);

            //如果它不可通过或者已经在关闭列表中，略过它。反之如下。
            //是否越界
            if (!(nextx < rmap.m_line && nextx >= 0)) {
                continue;
            }
            if (!(nexty < rmap.m_row && nexty >= 0)) {
                continue;
            }

            //是否是障碍物
            if (rmap.m_map[nextx][nexty] != RMap::RMAP_FREE && rmap.m_map[nextx][nexty] != RMap::RMAP_END_POINT &&
                rmap.m_map[nextx][nexty] != RMap::RMAP_START_POINT) {
                continue;
            }


            //是否在closelist中
            int isExistClose = 0;
            for (auto t: closelist) {
                if (t == next_point) {
                    isExistClose = 1;
                    break;
                }
            }
            if (isExistClose == 1) {
                continue;
            }



            //判断四面的某一个点是否在openlist里面

            int isExistOpen = 0;//开始假设不存在

            // 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。
            // 更低的G值意味着更好的路径。
            // 如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。
            // 如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
            for (auto &t: openlist) {
                //如果存在
                if (next_point == t) {
                    isExistOpen = 1;

                    int newG = next_point.G + calcG(cur_point, next_point);
                    int oldG = next_point.G + calcG(*(next_point.parent), next_point);
                    if (newG < oldG) {

                        next_point.parent = new point();
                        next_point.parent->x = cur_point.x;
                        next_point.parent->y = cur_point.y;
                        next_point.parent->F = cur_point.F;
                        next_point.parent->G = cur_point.G;
                        next_point.parent->H = cur_point.H;

                        next_point.parent->parent = cur_point.parent;

                    }

                }

            }

            //如果openlist不存在这个元素
            //如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
            if (isExistOpen == 0) {
                next_point.G = calcG(cur_point, next_point) + cur_point.G;
                next_point.H = manhattanDist(next_point, point(rmap.m_endX, rmap.m_endY));
                next_point.F = next_point.G + next_point.H;

                next_point.parent = new point();

                next_point.parent->x = cur_point.x;
                next_point.parent->y = cur_point.y;
                next_point.parent->F = cur_point.F;
                next_point.parent->G = cur_point.G;
                next_point.parent->H = cur_point.H;
                next_point.parent->parent = cur_point.parent;

                openlist.emplace_back(next_point);

            }// closest if end
        }// 8 side check end
    }//the body which check whether openlist has no element is ended
    return {};
}


#endif //UAV_ASTAR_H